using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using System.Diagnostics;
using System.Windows.Forms;
using System.IO;
using ScannerLib.Visualizer;
using System.Threading;
using ScannerLib.Tsai;

namespace ScannerLib.FrameGrabber
{
    public static class GridFinder
    {
        public static Bitmap GetEdges(string fileName, int edgeThreshold)
        {
            Process process = new Process();
            process.StartInfo.UseShellExecute = false;
            process.StartInfo.RedirectStandardError = true;
            process.StartInfo.Arguments = "-edge \"" + fileName + "\" " + edgeThreshold + " edgeMap.pgm";
            process.StartInfo.FileName = Path.GetDirectoryName(Application.ExecutablePath) + "\\" + "HoughTransforms.exe";
            process.Start();

            string errors = "";
            try
            {
                errors = process.StandardError.ReadToEnd();
            }
            catch (Exception ex)
            {
                if (!process.HasExited)
                    process.Kill();

            }

            if (process.ExitCode == -1)
            {
                throw new Exception("Choose different image or adjust threshholds - too many edges found");
            }
            else if (process.ExitCode != 0)
            {
                throw new Exception("Hough transform programm terminated unexpectedly: \n" + errors);
            }

            process.Dispose();

            return FrameWriter.ReadPGM(Path.GetDirectoryName(Application.ExecutablePath) + "\\" + "edgeMap.pgm");
        }

        private static bool IsProcessed(List<KeyValuePair<Point, Point>> processed, Point p1, Point p2)
        {
            return processed.FindIndex(delegate(KeyValuePair<Point, Point> kvp)
            {
                return (kvp.Key == p1 && kvp.Value == p2) || (kvp.Value == p1 && kvp.Key == p2);
            }) >=0;
        }

        private static KeyValuePair<Point,Point> Find(List<KeyValuePair<Point, Point>> processed, Point p1, Point p2)
        {
            return processed.Find(delegate(KeyValuePair<Point, Point> kvp)
            {
                return (kvp.Key == p1 && kvp.Value == p2) || (kvp.Value == p1 && kvp.Key == p2);
            });
        }

        public static List<CalibrationPoint> FindCalibrationPoints(Bitmap edgeImage, List<Point> houghPoints,
            double thetaSimilarityThreshold, double roSimilarityThreshold, LineHoughSettings HoughSettings, double gridCellSize)
        {
            List<KeyValuePair<Point, Point>> processed = new List<KeyValuePair<Point, Point>>();
            Dictionary<KeyValuePair<Point, Point>, CalibrationPoint> allCalibrationPoints = new Dictionary<KeyValuePair<Point, Point>, CalibrationPoint>();

            int caliPointNum = 0;
            foreach (Point p in houghPoints)
            {
                double theta = LineHoughSettings.RAD(p.X * 360.0 / HoughSettings.ThetaRes);
                double ro = p.Y * HoughSettings.RoRes;

                double sin = Math.Sin(theta), cos = -Math.Cos(theta);

                

                double vx = cos / ro, vy = sin / ro;

                if( Math.Abs(vx) < 0.000000001 && Math.Abs(vy) < 0.000000001)
                    continue;

                if (Double.IsNaN(ro) || Double.IsInfinity(ro) || Math.Abs(ro) < 0.0000000001)
                {
                    continue;
                }

                if (Double.IsNaN(theta) || Double.IsInfinity(theta) || Math.Abs(theta) < 0.0000000001)
                {
                    continue;
                }

                foreach (Point p1 in houghPoints)
                {
                    if (p1 == p || IsProcessed(processed, p, p1))
                        continue;

                    

                    KeyValuePair<Point, Point> straight = new KeyValuePair<Point, Point>(p,p1);

                    processed.Add(straight);

                    double theta1 = LineHoughSettings.RAD(p1.X * 360.0 / HoughSettings.ThetaRes);
                    double ro1 = p1.Y * HoughSettings.RoRes;
                    double sin1 = Math.Sin(theta1), cos1 = -Math.Cos(theta1);
                    double vx1 = cos1 / ro1, vy1 = sin1 / ro1;

                    if (Math.Abs(theta1 - theta) < thetaSimilarityThreshold)
                        continue;

                    if (Double.IsNaN(ro1) || Double.IsInfinity(ro1) || Math.Abs(ro1) < 0.0000000001)
                    {
                        continue;
                    }

                    if (Double.IsNaN(theta1) || Double.IsInfinity(theta1) || Math.Abs(theta1) < 0.0000000001)
                    {
                        continue;
                    }

                    if( Math.Abs(vx1) < 0.000000001 && Math.Abs(vy1) < 0.000000001)
                        continue;
                    double xs, ys;

                    if (Math.Abs(vx1) > 0.000000001)
                    {
                        if (Math.Abs(vy1 - vy * vx1 / vx) < 0.000000001)
                            continue; // parallel lines

                        ys = (1 - vx1 / vx) / (vy1 - vy * vx1 / vx);
                        xs = (1 - vy * ys) / vx;
                    }
                    else
                    {
                        continue;
                    }

                   
                    // make sure it's on screen                    
                    if (xs >= 0 && xs <= edgeImage.Width - 1 && ys >= 0 && ys <= edgeImage.Height - 1)
                    {
                        CalibrationPoint cp = new CalibrationPoint(++caliPointNum);
                        cp.X = xs;
                        cp.Y = ys;

                        if (edgeImage.GetPixel((int)Math.Floor(xs), (int)Math.Floor(ys)).R > 25)
                        {
                            double lx, ly, lx1, ly1, px,py,px1,py1;
                            
                            // check that point actually on those lines

                            lx = cos * ro;
                            ly = sin * ro;
                            lx1 = cos1 * ro1;
                            ly1 = sin1 * ro1;

                            px = xs - lx;
                            py = ys - ly;

                            px1 = xs - lx1;
                            py1 = ys - ly1;

                            if (Math.Abs(px1 * lx1 + py1 * ly1) > 0.00000001 || Math.Abs(px * lx + py * ly) > 0.00000001)
                                continue;

                            allCalibrationPoints[Find(processed, p, p1)] = cp;
                        }
                    }
                }
            }

            List<CalibrationPoint> finalPoints = new List<CalibrationPoint>();
            foreach (CalibrationPoint cp in allCalibrationPoints.Values)
                finalPoints.Add(cp);

            List<List<CalibrationPoint>> pointGroups = new List<List<CalibrationPoint>>();

            foreach (CalibrationPoint cp in finalPoints)
            {
                List<List<CalibrationPoint>> groupsToMerge = new List<List<CalibrationPoint>>();
                bool added = false;
                foreach (List<CalibrationPoint> mergeGroup in pointGroups)
                {
                    foreach (CalibrationPoint mergePoint in mergeGroup)
                    {
                        if ((mergePoint.X - cp.X) * (mergePoint.X - cp.X) + (mergePoint.Y - cp.Y) * (mergePoint.Y - cp.Y) < roSimilarityThreshold * roSimilarityThreshold)
                        {
                            groupsToMerge.Add(mergeGroup);
                            break;
                        }
                    }
                }

                if (groupsToMerge.Count > 0)
                {
                    groupsToMerge[0].Add(cp);

                    for (int i = 1; i < groupsToMerge.Count; i++)
                    {
                        pointGroups.Remove(groupsToMerge[i]);
                        groupsToMerge[0].AddRange(groupsToMerge[i]);
                    }
                }
                else
                {
                    List<CalibrationPoint> l = new List<CalibrationPoint>();
                    l.Add(cp);
                    pointGroups.Add(l);
                }
            }

            finalPoints.Clear();

            caliPointNum = 0;
            foreach (List<CalibrationPoint> mergeGroup in pointGroups)
            {
                double sx = 0, sy = 0;

                foreach (CalibrationPoint cp in mergeGroup)
                {
                    sx += cp.X;
                    sy += cp.Y;
                }

                sx = sx / mergeGroup.Count;
                sy = sy / mergeGroup.Count;

                CalibrationPoint cpNew = new CalibrationPoint(++caliPointNum);
                cpNew.X = sx;
                cpNew.Y = sy;
                finalPoints.Add(cpNew);
            }

            // now assign distances

            List<CalibrationPoint> processedPoints = new List<CalibrationPoint>();
            CalibrationPoint origin = FindOrigin(finalPoints);
            CalibrationPoint previousPoint = origin;
            CalibrationPoint nextPoint = null;
            
            processedPoints.Add(origin);
            finalPoints.Remove(origin);

            int x = 0, y = 0;

            while (finalPoints.Count > 0)
            {
                nextPoint = FindNextPoint(finalPoints, previousPoint);

                if (nextPoint == null)
                {
                    x = 0;
                    previousPoint = FindOrigin(finalPoints);
                    finalPoints.Remove(previousPoint);

                    while (previousPoint.X - origin.X > 2)
                    {
                        previousPoint = FindOrigin(finalPoints);
                        if (previousPoint == null)
                            break;

                        finalPoints.Remove(previousPoint);
                       
                    }

                    if (previousPoint == null)
                        break;

                    origin = previousPoint;

                    previousPoint.PY = (++y) * gridCellSize;
                    previousPoint.PX = 0;
                    processedPoints.Add(previousPoint);
                }
                else
                {
                    previousPoint = nextPoint;
                    finalPoints.Remove(nextPoint);
                    nextPoint.PX = (++x) * gridCellSize;
                    nextPoint.PY = y * gridCellSize;
                    processedPoints.Add(nextPoint);
                }

            }
            
            return processedPoints;
        }

        private static CalibrationPoint FindNextPoint(List<CalibrationPoint> finalPoints, CalibrationPoint previousPoint)
        {
            CalibrationPoint nextPoint = null;
            foreach (CalibrationPoint cp in finalPoints)
            {
                if (cp.X < previousPoint.X)
                    continue;

                if (nextPoint == null)
                    nextPoint = cp;
                else
                {
                    if (Math.Abs(cp.Y - nextPoint.Y) < 2)
                    {
                        if (cp.X < nextPoint.X)
                            nextPoint = cp;
                    }
                    else if (cp.Y > nextPoint.Y)
                    {
                        nextPoint = cp;
                    }
                }
            }
            return nextPoint;
        }

        private static CalibrationPoint FindOrigin(List<CalibrationPoint> finalPoints)
        {
            CalibrationPoint origin = null;

            foreach (CalibrationPoint cp in finalPoints)
            {
                if (origin == null)
                    origin = cp;
                else
                {
                    if (Math.Abs(cp.Y - origin.Y) < 2)
                    {
                        if (cp.X < origin.X)
                        {
                            origin = cp;
                        }
                    }
                    if (cp.Y > origin.Y)
                        origin = cp;

                }
            }
            return origin;
        }

        public static List<PointF> PruneHoughPoints(List<Point> houghPoints, double thetaSimilarityThreshold, double roSimilarityThreshold, LineHoughSettings HoughSettings)
        {
            List<PointF> finalPoints = new List<PointF>();

            List<Point> prunedHoughPoints = new List<Point>();

            Dictionary<Point, List<Point>> houghGroups = new Dictionary<Point, List<Point>>();
            foreach (Point p in houghPoints)
            {
                int i = 0;
                for (i = 0; i < prunedHoughPoints.Count; i++)
                {
                    double theta = LineHoughSettings.RAD(p.X * 360.0 / HoughSettings.ThetaRes);
                    double ro = p.Y * HoughSettings.RoRes;

                    double theta1 = LineHoughSettings.RAD(prunedHoughPoints[i].X * 360.0 / HoughSettings.ThetaRes);
                    double ro1 = prunedHoughPoints[i].Y * HoughSettings.RoRes;

                    if (Math.Abs(theta1 - theta) < thetaSimilarityThreshold && Math.Abs(ro1 - ro) < roSimilarityThreshold)
                    {
                        houghGroups[prunedHoughPoints[i]].Add(p);
                        break;
                    }
                }

                if (i == prunedHoughPoints.Count)
                {
                    prunedHoughPoints.Add(p);
                    houghGroups[p] = new List<Point>();
                    houghGroups[p].Add(p);
                }
            }

            foreach (Point _p in prunedHoughPoints)
            {
                List<Point> houghGroup = houghGroups[_p];

                float sumT = 0, sumR = 0;

                foreach (Point p in houghGroup)
                {
                    sumT += p.X;
                    sumR += p.Y;
                }

                finalPoints.Add(new PointF(sumT / houghGroup.Count, sumR / houghGroup.Count));
            }
            return finalPoints;
        }

        public static Bitmap GetHough(string fileName, int edgeThreshold, int houghThreshold)
        {
            Process process = new Process();
            process.StartInfo.UseShellExecute = false;
            process.StartInfo.RedirectStandardError = true;
            process.StartInfo.Arguments = "-hough \"" + fileName + "\" " + edgeThreshold + " " + houghThreshold +  " hough.pgm";
            process.StartInfo.FileName = Path.GetDirectoryName(Application.ExecutablePath) + "\\" + "HoughTransforms.exe";
            process.Start();

            string errors = "";
            try
            {
                errors = process.StandardError.ReadToEnd();
            }
            catch (Exception ex)
            {
                if (!process.HasExited)
                    process.Kill();

            }

            if (process.ExitCode == -1)
            {
                throw new Exception("Choose different image or adjust threshholds - too many edges found");
            }
            else if (process.ExitCode != 0)
            {
                throw new Exception("Hough transform programm terminated unexpectedly: \n" + errors);
            }

            process.Dispose();

            return FrameWriter.ReadPGM(Path.GetDirectoryName(Application.ExecutablePath) + "\\" + "hough.pgm");
        }

        public static KeyValuePair<LineHoughSettings,  List<Point>> FindGrid(string fileName, int edgeThreshold, int houghThreshold)
        {
            Process process = new Process();
            process.StartInfo.UseShellExecute = false;
            process.StartInfo.RedirectStandardOutput = true;
            process.StartInfo.Arguments = "-fg " + fileName + " " + edgeThreshold + " " + houghThreshold;
            process.StartInfo.FileName = Path.GetDirectoryName(Application.ExecutablePath) + "\\" + "HoughTransforms.exe";
            process.Start();

            LineHoughSettings houghSettings = null;
            List<Point> points = null;

            try
            {
                string settings = process.StandardOutput.ReadLine();
                string [] spSettings = settings.Split(new char [] { ' '},StringSplitOptions.RemoveEmptyEntries);

                houghSettings = new LineHoughSettings(Convert.ToInt32(spSettings[1]),Convert.ToInt32(spSettings[0]));
                points = new List<Point>();
                while((settings = process.StandardOutput.ReadLine()) != null)
                {
                    spSettings = settings.Split(new char [] { ' '},StringSplitOptions.RemoveEmptyEntries);
                    if (spSettings.Length > 0 )
                    {
                        points.Add(new Point(Convert.ToInt32(spSettings[1]),Convert.ToInt32(spSettings[0])));
                    }
                }

            }
            catch(Exception ex)
            {
                if( !process.HasExited )
                    process.Kill();

            }

            if( process.ExitCode == -1 )
            {
                throw new Exception("Choose different image or adjust threshholds - too many edges found");
            } else if(process.ExitCode != 0 )
            {
                throw new Exception("Hough transform programm terminated unexpectedly");
            }

            process.Dispose();

            
           return new KeyValuePair<LineHoughSettings,List<Point>>(houghSettings,points);
        }
    }
}
